#include "ParticleSystem.h"

void ParticleSystem::Simulation() {

	BT_PROFILE("ParticleSystemSimulation");

	float residualDt = timeStepSize;
	while (residualDt > 0.0f) {
		if (residualDt < maxSubTimeStepSize) dt = residualDt;
		else dt = maxSubTimeStepSize;
		residualDt -= dt;
		Advection();
	//	DetectCollisionWithParticle();
		Neighboring();
		ComputeRho();
		ComputeMoment();
		ComputeForceAndVelocity();
	}
}

void ParticleSystem::renderParticle() {
	renderParticle(true);
}

void ParticleSystem::renderParticle(bool drawArrow) {

	BT_PROFILE("RenderParticles");

	glPushMatrix();
	glPushAttrib( GL_POLYGON_BIT ); // includes GL_CULL_FACE
	glDisable(GL_CULL_FACE); // draw from all sides
	glPointSize(3.0f);
	

	glBegin(GL_POINTS);
	for (int i=0; i<numParticle; i++) {
		glColor3f(pos[i].x(), pos[i].y(), 1.0);
		Vector3 position = (worldPos[i] = pos[i] * scalingFactor + ParticleSystemPosition);
		glVertex3f(position.x(), position.y(), position.z());
	}
	glEnd();

	if (drawArrow) {
		for (int i=0; i<numParticle; i++) {
			Vector3 position = pos[i] * scalingFactor;
	//		DrawArrow(position, position + prs[i].normalized() * 0.1f, btVector3(0.2, 1.0, 0.2));
	//		DrawArrow(position, position + vis[i].normalized() * 0.1f, btVector3(1.0, 0.2, 0.2));
	//		DrawArrow(position, position + mfs[i].normalized() * 0.1f, btVector3(0.2, 0.2, 1.0));
	//		DrawArrow(position, position + frs[i].normalized() * 0.1f, btVector3(0.7, 0.2, 0.7));
			DrawArrow(position, position + tof[i].normalized() * 0.1f, btVector3(0.6, 0.9, 0.2));
	//		DrawArrow(position, position + Vector3(0, contactID[i] * 0.1, 0), btVector3(0.1, 0.6, 0.9));
		}
	}

	glPopAttrib(); // GL_CULL_FACE
	glPopMatrix();
}

void ParticleSystem::ParticleSeeding() {

	Vector3 startPos = Vector3(0.0, 0.0, 0.0);
	//float gap = h * 0.69;
	//float gap = 1.0f / 32.0f;
	int cnt = 0;
	int sline = pow(numParticle, 1./2.);
	float gap = domain.x() / sline;
	sline++;
	//sline = min(sline, (int)(1. / gap));
	for (int i=0; i<sline; i++) {
		//for (int j=0; j<sline; j++) {
			for (int k=0; k<sline; k++) {
				Vector3 tmpPos, tmpVel;
				tmpPos = startPos + Vector3(i, 0.5, k) * gap;
				tmpVel = Vector3::RandomVector(-0.01, 0.01);
				pos.push_back(tmpPos);
				vel.push_back(tmpVel);
				mm.push_back(Vector3(0.0, 0.0, 0.0));
				worldPos.push_back(pos[i] * scalingFactor + ParticleSystemPosition);
				rho.push_back(0.0);
				pressure.push_back(0.0);
				prs.push_back(Vector3(0.0, 0.0, 0.0));
				vis.push_back(Vector3(0.0, 0.0, 0.0));
				mfs.push_back(Vector3(0.0, 0.0, 0.0));
				frs.push_back(Vector3(0.0, 0.0, 0.0));
				tof.push_back(Vector3(0.0, 0.0, 0.0));
				contactID.push_back(cnt);

				cnt++;
				if (cnt == numParticle) return;
			}
		//}
	}
	numParticle=cnt;
}

void ParticleSystem::Advection() {

	BT_PROFILE("Advection");

	GetAabbFromWorld();
	gridMap.Clear();

	for (int i=0; i<numParticle; i++) {
		Vector3 prePos = pos[i];

		vel[i] *= fixAxis;

		pos[i] += vel[i]*dt;

		worldPos[i] = pos[i] * scalingFactor + ParticleSystemPosition;

		DetectCollision(prePos, pos[i], vel[i]);

		SpatialGrid(pos[i], i);
	}

}

void ParticleSystem::GetAabbFromWorld() {

	BT_PROFILE("GetAabbFromWorld");
	numObjects=0;
	aabbs[0].clear();
	aabbs[1].clear();

	for (int i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btVector3 min, max;
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body)
		{
			body->getAabb(min, max);
			aabbs[0].push_back( (min-ParticleSystemPosition) / scalingFactor * 1.003 );
			aabbs[1].push_back( (max-ParticleSystemPosition) / scalingFactor * 1.003 );
			numObjects++;
		}
	}
}

void ParticleSystem::DetectCollision(Vector3& prePos, Vector3& pos, Vector3& vel) {

	BT_PROFILE("DetectCollision");
	/*
	btVector3 worldPrePos = ParticleSystemPosition + prePos * scalingFactor;
	btVector3 worldPos = ParticleSystemPosition + pos * scalingFactor;
	btVector3 vel_ = vel * scalingFactor;

	btVector3 start = worldPrePos;
	btVector3 end = worldPos;

	btCollisionWorld::ClosestRayResultCallback cb(start, end);
	m_dynamicsWorld->rayTest(start, end, cb);

	if (cb.hasHit()) {
	Vector3 hitPoint = cb.m_hitPointWorld / scalingFactor;
	Vector3 hitNormal = cb.m_hitNormalWorld.normalized();
	if ( (prePos-hitPoint).dot(hitNormal) * (pos-hitPoint).dot(hitNormal) <= 0.0f) {
	pos = hitPoint + hitNormal*0.0001f;
	if ( vel.dot(hitNormal) <= 0.0f) {
	vel -= vel.dot(hitNormal) * hitNormal;
	}
	}
	}
	*/

	// collision with bullet object(rigidbody)
	for (int i=0; i<numObjects; i++) {
		if (pos > aabbs[0][i] && pos < aabbs[1][i]) {
			// collision?

			Vector3 aabb;
			int dir=1;
			if (prePos > aabbs[0][i]) 
				aabb=aabbs[1][i];		// contact at higher face
			else {
				aabb=aabbs[0][i];		// contact at lower face
				dir=-1;
			}

			for (int j=0; j<3; j++) {
				if ( (prePos[j] > aabb[j]) ^ (pos[j] > aabb[j]) ) {

					Vector3 n;
					n[j]=dir;
					float temp_dt = (aabb[j]-prePos[j]) / vel[j];
					if (temp_dt > dt) temp_dt = dt;
					temp_dt *= 0.99;
					pos = prePos + vel * temp_dt;
					pos[j] += 0.0001 * dir;
					temp_dt = dt-temp_dt;
					vel = vel - (vel.dot(n)) * n * 1.02;
					pos = pos + vel * temp_dt;
					break;
				}
			}
		}
	}


	// boundary
	if (pos.x() < 0.0) {pos.x() = 0.0; vel.x() = 0.0;}
	if (pos.y() < 0.0) {pos.y() = 0.0; vel.y() = 0.0;}
	if (pos.z() < 0.0) {pos.z() = 0.0; vel.z() = 0.0;}

	if (pos.x() > domain.x()) {pos.x() = domain.x(); vel.x() = 0.0;}
	if (pos.y() > domain.y()) {pos.y() = domain.y(); vel.y() = 0.0;}
	if (pos.z() > domain.z()) {pos.z() = domain.z(); vel.z() = 0.0;}

}

void ParticleSystem::DetectCollisionWithParticle() {
	BT_PROFILE("DetectCollisionWithParticle");
	
	for (int i=0; i<numParticle; i++)
		contactID[i] = i;

	for (int i=0; i<numParticle; i++) {
		if (neighborMap.isInserted(i)) {
			vector<int> neighborOfi = neighborMap[i];
			int numOfNeighbors = neighborOfi.size();
			for (int ci=0; ci<numOfNeighbors ; ci++) {
				int j = neighborOfi[ci];
				Vector3 dir = pos[i] - pos[j];
				float distance = dir.length();
				if (distance < radius) {
					float move = (radius_ - distance) ;
					if (move < 0.0) move = 0.0;
					dir.normalize();
					dir *= move;
					pos[i]+= dir;
					pos[j]-= dir;
					vel[i]*= 0.0;
					vel[j]*= 0.0;
					contactID[j] = contactID[i];
					break;
				}
			}
		}
	}
}

void ParticleSystem::SpatialGrid(Vector3& pos, int index) {
	BT_PROFILE("SpatialGrid");
	gridMap.Add(GridIndex(pos/h), index);

}

void ParticleSystem::Neighboring() {

	BT_PROFILE("Neighboring");

	neighborMap.Clear();

	float radius2 = radius * radius;

	for (int i=0; i<numParticle; i++) {
		int x = (int) (pos[i].x()/h);
		int y = (int) (pos[i].y()/h);
		int z = (int) (pos[i].z()/h);

		if (x>=numXgrid) x = numXgrid - 1;
		if (y>=numYgrid) y = numYgrid - 1;
		if (z>=numZgrid) z = numZgrid - 1;

		for (int dx=-1; dx<2; dx++) {
			int nx = x+dx;
			if (nx<0 || nx>=numXgrid) continue;
			for (int dz=-1; dz<2; dz++) {
				int nz = z+dz;
				if (nz<0 || nz>=numZgrid) continue;
				for (int dy=-1; dy<2; dy++) {
					int ny = y+dy;
					if (ny<0 || ny>=numYgrid) continue;

					int index = GridIndex(nx, ny, nz);
					if (!gridMap.isInserted(index)) continue;
					vector<int> cell = gridMap[index];
					int cnt = cell.size();
					for (int ci=0; ci<cnt; ci++) {
						int j = cell[ci];
						if (i==j) continue;
						Vector3 dir = pos[i] - pos[j];
						float distance2 = dir.length_square();
						if (distance2 < h2) neighborMap.Add(i, j);
						/*if (distance2 < radius2 ) {
							float move = (sqrt(radius2) - sqrt(distance2)) * 0.499;
							move = sqrt(move);
							dir.normalize();
							dir *= move;
							pos[i]+= dir;
							pos[j]-= dir;
							vel[i]*= 0.0;
							vel[j]*= 0.0;
							contactID[j] = contactID[i];
						}*/
					}
				}
			}
		}
	}
}

void ParticleSystem::ComputeRho() {

	BT_PROFILE("ComputeRho");

	for (int i=0; i<numParticle; i++) {
		Vector3 pi = pos[i];
		float tempRho = pow(h,6);
		if (neighborMap.isInserted(i)) {
			vector<int> neighborOfi = neighborMap[i];
			int numOfNeighbors = neighborOfi.size();
			for (int ci=0; ci<numOfNeighbors; ci++) {
				Vector3 pj = pos[neighborOfi[ci]];
				float distance2 = (pi-pj).length_square();
				tempRho += (float)pow(h2-distance2, 3);
			}
		}
		tempRho *= m* constantOfPoly6;
		if (tempRho < rho0*0.8f) {
			tempRho = rho0;
		}
		else if (tempRho < rho0) {
			tempRho = rho0*sigma;
		}
		rho[i] = tempRho;
		pressure[i] = k * (rho[i] - rho0);
	}
}

void ParticleSystem::ComputeMoment() {

	BT_PROFILE("ComputeMoment");

	for (int i=0; i<numParticle; i++) {
		Vector3 pi = worldPos[i];
		Vector3 b = Magnet::CalcMField(NULL, pi);
		Vector3 mi = induceFactor * b * m / rho0;
		Vector3 b2;
		if (neighborMap.isInserted(i)) {
			vector<int> neighborOfi = neighborMap[i];
			int numOfNeighbors = neighborOfi.size();
			for (int ci=0; ci<numOfNeighbors; ci++) {
				Vector3 pj = worldPos[neighborOfi[ci]];
				float distance = (pj-pi).length();
				Vector3 n = (pj-pi).normalized();
				b2 += ( 3*n*(n.dot(mi)) - mi) * pow(distance,-3);
			}
		}
		b2 *= Magnet::k;
		mm[i] = mi *3 + induceFactor * b2 * m / rho[i];
	
	}
}

void ParticleSystem::ComputeForceAndVelocity() {
	BT_PROFILE("ComputeForceAndVelocity");

	for (int i=0; i<numParticle; i++) {
		Vector3 posi = pos[i];
		Vector3 veli = vel[i];
		Vector3 mi = mm[i];
		Vector3 pressureForce;
		Vector3 viscosityForce;
		Vector3 magnetForce;
		float rhoi = rho[i];
		float pressurei = pressure[i];

		if (neighborMap.isInserted(i)) {
			vector<int> neighborOfi = neighborMap[i];
			int numOfNeighbors = neighborOfi.size();
			for (int ci=0; ci<numOfNeighbors; ci++) {
				BT_PROFILE("Kernel");
				int j = neighborOfi[ci];
				Vector3 posj = pos[j];
				Vector3 velj = vel[j];
				Vector3 mk = mm[j];
				float rhoj = rho[j];
				float pressurej = pressure[j];
				Vector3 r = posi - posj;
				Vector3 n = r.normalized();
				float distance = r.length();
				pressureForce -= ((pressurei + pressurej) / (2.0f * rhoj)) * (float) pow(h-distance,2) * (r/distance);
				//viscosityForce += (velj - veli) * (h-distance) / rhoj;

				// magnetic force between particle
				if (contactID[i] == contactID[j]) 
					continue;
				Vector3 mfc = -5 * n * mk.dot(n) * mi.dot(n);
				mfc += n * mk.dot(mi);
				mfc += mk * mi.dot(n) + mi * mk.dot(n);
				mfc *= 3;
				mfc *= pow(distance, -4);
				mfc.limitLength(1.0);
				magnetForce += mfc * rhoj;
			}
		}

		// magnetic force from magnet
		pressureForce *= m * constantOfGradientSpiky ;
		viscosityForce*= mu * m* constantOfLaplacianViscosity;
		magnetForce *= k * 7000;

		//magnetForce += Magnet::GetMagnetForce(posi, mi) * 60000000;

		prs[i] = pressureForce;
		vis[i] = viscosityForce;
		mfs[i] = magnetForce;

		externalForce = gravity;

		Vector3 force = rhoi*externalForce + pressureForce + viscosityForce + magnetForce;

		if (vel[i].length_square() < 0.00000000001) {	// static friction
			if (force.length_square() < maxStaticFriction2 * rhoi * rhoi)
				force = 0;

		}
		else {									// moving particle - dynamic friction
			force += (frs[i] = -vel[i].normalized() * rhoi * dynamicStatic);
		}

		Vector3 acceleration = force / rhoi;
		vel[i] += acceleration * dt;

		if (vel[i].length() > maxSpeed) {
			vel[i].normalize();
			vel[i]*=maxSpeed;
		}
		/*
		if (vel[i].length() > 5.0f) {
			vel[i]
		}*/

		vel[i] *= 0.999f;

		tof[i] = force;
	}
}

int ParticleSystem::GridIndex(const Vector3& v) {
	BT_PROFILE("GridIndex");
	return GridIndex((int)v.x(), (int)v.y(), (int)v.z());
}
int ParticleSystem::GridIndex(int x, int z) {
	BT_PROFILE("GridIndex");
	if (x>=numXgrid) x=numXgrid-1;
	if (z>=numZgrid) z=numZgrid-1;
	return x + numXgrid * z;
}
int ParticleSystem::GridIndex(int x, int y, int z) {
	BT_PROFILE("GridIndex");
	if (x>=numXgrid) x=numXgrid-1;
	if (y>=numYgrid) y=numYgrid-1;
	if (z>=numZgrid) z=numZgrid-1;
	return x + numXgrid * y + numXgrid * numYgrid * z;
}

const float B_spline_kernel(Vector3 xi, Vector3 xj, float h) {

	float R = (xi-xj).length() / h;
	float C = 1.5 / PI * pow(h, -3);
	float R2 = R*R;
	if (R >=0 && R < 1)
		return 1.5 - R2 + R2*R/2.0;
	else if (R >= 1 && R < 2)
		return pow((2-R),2) / 6.0;
	else
		return 0.0;
}